#include "StdAfx.h"
#include "ConvexHull.h"


ConvexHull::ConvexHull(void) : convexHull(0)
{
	ID = -1;
}


ConvexHull::~ConvexHull(void)
{
}

bool ConvexHull::IsInside(const btVector3& p, bool allowOnPlane)
{
	//std::cout << "\n\t\tIsInside (" << vertices.size() << ")\t" << planes.size();
	//Triangle::Print(p);
	
	if(this->vertices.size() < 2)
		return false;


	//std::cout << "\n\t";
	//Triangle::Print(p);
	//std::cout << " inside: ";
	if(planes.size() == 0)
		this->ComputePlanes();

	if(planes.size() == 0)
		return false;

	for(int i = 0; i < planes.size(); ++i)
	{
		if(DistPointPlane(p, planes[i]) > 0.0f)
			return false;
	}

	if(allowOnPlane)
	{
		for(int i = 0; i < planes.size(); ++i)
		{
			if(p.dot(planes[i].normal) == planes[i].dist)
			{
				//std::cout << "false" << std::endl;
				return false;
			}
		}
	}

	return true;
	//std::cout << "\tPLanes: " << planes.size() << "\t";
	btAlignedObjectArray<btVector3> list;
	list.push_back(p);

	//for(int i = 0; i < planes.size(); ++i)
	//{
	//	
	//	/*std::cout << "\n\t\t\t";
	//	Triangle::Print(planes[i].normal);*/
	//	
	//	if(btGeometryUtil::areVerticesBehindPlane(planes[i].normal, list, 0.01f) == false)
	//	{
	//		return false;
	//	}
	//	else if(allowOnPlane)
	//	{
	//		if(p.dot(planes[i].normal) == planes[i].dist)
	//		{
	//			//std::cout << "false" << std::endl;
	//			return false;
	//		}
	//	}

	//}
	//std::cout << result << std::endl;
	return true;
}

bool ConvexHull::operator==(const ConvexHull& other)
{
	if(other.vertices.size() == this->vertices.size() && this->vertices.size() > 0)
	{
		int n = this->vertices.size();
		bool result = true;

		for(int i = 0; i < n; ++i)
		{
			result &= other.vertices.findLinearSearch(this->vertices[i]) < n;
		}
		return result;
	}
	return false;
}

bool ConvexHull::RayTest(const btVector3& origin, const btVector3& destination, btAlignedObjectArray<btVector3>& hitPoints, bool excludeEnds)
{
	btVector3 offset = destination - origin;
	for(int j = 0; j < planes.size(); ++j)
	{
		if(planes[j].normal.dot(origin) == planes[j].dist && planes[j].normal.dot(destination) == planes[j].dist)
			return false;

		btScalar numerator = (planes[j].dist - planes[j].normal.dot(origin));
		btScalar denominator = planes[j].normal.dot(offset);
		if(denominator == 0.0f)
			continue;
		
		btScalar t = numerator / denominator;
		//std::cout << "\tt: " << t;
		bool calculate = false;
		if(excludeEnds)
		{
			if(t > 0.f && t < 1.f)
				calculate = true;
		}
		else if(t >= 0.0f && t <= 1.0f)
			calculate = true;

		if(calculate)
		{
			btVector3 q = origin + t * offset;
			if(this->IsInside(q) && this->vertices.findLinearSearch(q) == this->vertices.size())
				hitPoints.push_back(q);
		}
	}	
		
	return hitPoints.size() > 0;
}

void ConvexHull::GetEdgesFromFace(int i, btAlignedObjectArray<btVector3>& outList)
{
	if(this->faces.size() == 0)
		return;

	if(i < 0)
		i = abs(i);
	i = i % this->faces.size();
	outList.clear();

	const Edge* start = &(this->edges[this->faces[i]]);
	const Edge* current = start;

	do
	{
		outList.push_back(this->vertices[current->getSourceVertex()]);
		outList.push_back(this->vertices[current->getTargetVertex()]);		

		current = (current->getNextEdgeOfFace());
	}
	while(start != current);
}

void ConvexHull::GetVerticesFromFace(int i, btAlignedObjectArray<btVector3>& outList)
{
	if(this->faces.size() == 0)
		return;

	if(i < 0)
		i = abs(i);
	i = i % this->faces.size();
	outList.clear();
	const Edge* start = &(this->edges[this->faces[i]]);
	const Edge* current = start;

	do
	{
		btVector3& tmp = this->vertices[current->getSourceVertex()];

		if(outList.findLinearSearch(tmp) == outList.size())
			outList.push_back(tmp);

		current = (current->getNextEdgeOfFace());
	}
	while(start != current);
}

bool ConvexHull::OnEdge(const btVector3& p)
{
	for(int i = 0; i < this->edges.size(); i+=2)
	{
		btVector3 a = this->vertices[this->edges[i].getSourceVertex()];
		btVector3 b = this->vertices[this->edges[i].getTargetVertex()];

		if(p.distance2(a) + p.distance2(b) > a.distance2(b))
			return true;
	}

	return false;
}

bool ConvexHull::RayTest(const btVector3& origin, const btVector3& destination, bool excludeEnds)
{
	btAlignedObjectArray<btVector3> list;
	return this->RayTest(origin, destination, list, excludeEnds);
}

void ConvexHull::ComputePlanes()
{
	//std::cout << "Plane compting:\n\tInitial vertices: "<< std::endl;
	//for(int i = 0; i < vertices.size(); ++i)
	//{
	//	std::cout << "\t\t";
	//	Triangle::Print(vertices[i]);
	//	std::cout << std::endl;
	//}
	this->planes.clear();

	for(int i = 0; i < this->faces.size(); ++i)
	{
		btAlignedObjectArray<btVector3> faceVertices, planeEquations;
		const Edge* start = &(this->edges[this->faces[i]]);
		const Edge* current = start;

		do
		{
			btVector3& tmp = this->vertices[current->getSourceVertex()];

			if(faceVertices.findLinearSearch(tmp) == faceVertices.size())
				faceVertices.push_back(tmp);

			current = (current->getNextEdgeOfFace());
		}
		while(start != current);
		//std::cout << "\tFace vertices (" << faceVertices.size() << ")" << std::endl;
		//for(int j = 0; j < faceVertices.size(); ++j)
		//{
		//	std::cout << "\t\t";
		//	Triangle::Print(faceVertices[j]);
		//	std::cout << std::endl;
		//}
		btGeometryUtil::getPlaneEquationsFromVertices(faceVertices, planeEquations);
		//std::cout << "\tComputed normals:" << std::endl;
		//for(int j = 0; j < planeEquations.size(); ++j)
		//{
		//	std::cout << "\t\t";
		//	Triangle::Print(planeEquations[j]);
		//	std::cout << std::endl;
		//}

		if(planeEquations.size() > 0)
		{
			//std::cout << "\n\t\tFace computation for: " <<  this << "\t" << faceVertices.size() << "\t" << planeEquations.size() << std::endl;
			btVector3 normal = planeEquations[0];
			if(fabs(normal.getX()) == 0.f)
				normal.setX(0.f);
			if(fabs(normal.getY()) == 0.f)
				normal.setY(0.f);
			if(fabs(normal.getZ()) == 0.f)
				normal.setZ(0.f);
			
			planes.push_back(btPlane(normal, normal.dot(faceVertices[0])));
		}
		
	}
	//std::cout << "Planes: " << this->planes.size() << std::endl;
}

float ConvexHull::DistPointPlane(const btVector3& p, const btPlane& plane)
{
	return ( plane.normal.dot(p) - plane.dist ) / plane.normal.dot( plane.normal );
}

bool ConvexHull::Intersect(ConvexHull& hull)
{
	/*std::cout << "\n\t\tHull collision" << std::endl;

	for(int i = 0; i < vertices.size(); ++i)
	{
		std::cout << "\t\t";
		Triangle::Print(vertices[i]);
		std::cout << std::endl;
	}

	std::cout <<"\t\tvs" << std::endl;

	for(int j = 0; j < hull.vertices.size(); ++j)
	{
		std::cout << "\t\t";
		Triangle::Print(hull.vertices[j]);
		std::cout << std::endl;
	}*/
	int count = 0;
	for(int i = 0; i < vertices.size(); ++i)
	{
		if(hull.vertices.findLinearSearch(vertices[i]) < hull.vertices.size())
			continue;

		if(hull.IsInside(vertices[i], true))
		{
			//std::cout << "\t\tIsInside hull"; 
			//Triangle::Print(vertices[i]);
			//std::cout << std::endl;			
			return true;
		}		
	}

	if(count > 1)
		return true;

	count = 0;

	for(int i = 0; i < hull.vertices.size(); ++ i)
	{
		if(vertices.findLinearSearch(hull.vertices[i]) < vertices.size())
			continue;
		if(this->IsInside(hull.vertices[i], true))
		{
			//std::cout << "\t\tHull is inside";
			//Triangle::Print(hull.vertices[i]);
			//std::cout << std::endl;
			return true;
		}
	}

	for(int i = 0; i < this->edges.size(); i+=2)
	{
		const btVector3 A = this->vertices[this->edges[i].getSourceVertex()];
		const btVector3 B = this->vertices[this->edges[i].getTargetVertex()];
		
		btVector3 vec1 (B - A);

		for(int j = 0; j < hull.edges.size(); j+=2)
		{
			const btVector3 C = hull.vertices[hull.edges[j].getSourceVertex()];
			const btVector3 D = hull.vertices[hull.edges[j].getTargetVertex()];

			btVector3 vec2(D - C);

			if( (A == C || A == D) && (B == C || B == D))
				continue;

			//if(vec1.length() * vec2.length() * sinf(vec1.angle(vec2)) == 0)	// rownolegle
			//	continue;
			float s,t;
			btVector3 C1, C2;
			float distance = this->ClosestPointSegmentSegment(A,B,C,D,s,t, C1, C2);
			
			if(C1 == C2 && C1 != A && C1 != B && (this->vertices.findLinearSearch(C1) == this->vertices.size() && hull.vertices.findLinearSearch(C1) == hull.vertices.size()))
			{
				/*std::cout << "\n\t\t\t";
				Triangle::Print(A);
				std::cout << "\t";
				Triangle::Print(B);
				std::cout << "\tvs\t";
				Triangle::Print(C);
				std::cout << "\t";
				Triangle::Print(D);
				std::cout << "\t: ";
				Triangle::Print(C1);
				std::cout << "\t";
				Triangle::Print(C2);
				std::cout << "\t" << s << "\t" << t << "\tdistance: " << distance;
				std::cout << std::endl;*/
				++count;
			}
		}
	}
	//return false;

	if(count > 1)
	{
		std::cout << count;
		return true;
	}

	//std::cout << "\tHull intersection" << std::endl;
	

	return false;
}

float ConvexHull::clamp(float value, float min, float max)
{
	if(value < min) return min;
	if(value > max) return max;
	return value;
}

float ConvexHull::ClosestPointSegmentSegment(const btVector3& p1, const btVector3& q1, const btVector3& p2, const btVector3& q2, float& s, float& t, btVector3& c1, btVector3& c2)
{
	btVector3 d1(q1 - p1);
	btVector3 d2(q2 - p2);
	btVector3 r(p1 - p2);
	float a = d1.dot(d1);
	float e = d2.dot(d2);
	float f = d2.dot(r);

	static float EPSILON = 0.01f;

	if(a <= EPSILON && e <= EPSILON)
	{
		s = t = 0.0f;
		c1 = p1;
		c2 = p2;
		return c1.distance2(c2);
	}

	if(a <= EPSILON)
	{
		s = 0.0f;
		t = f/e;
		t = this->clamp(t, 0, 1);
	}
	else
	{
		float c = d1.dot(r);
		if(e <= EPSILON)
		{
			t = 0;
			s = clamp(-c/a, 0, 1);
		}
		else
		{
			float b = d1.dot(d2);
			float denom = a*e-b*b;
			if(denom != 0.0f)
			{
				s = clamp((b*f - c*e)/denom, 0, 1);
			}
			else
			{
				s = 0;
			}

			t = (b*s+f)/e;
			if(t < 0.f)
			{
				t = 0;
				s = clamp(-c / a, 0, 1);
			}
			else if(t > 1.f)
			{
				t = 1.f;
				s = clamp((b - c) / a, 0, 1);
			}
		}
	}

	c1 = p1 + d1 * s;
	c2 = p2 + d2 * t;
	return c1.distance(c2);
}